import rclpy
from rclpy.node import Node
from tf2_ros import TransformListener, Buffer
from tf_transformations import euler_from_quaternion
import math


# 本节点用于监听TF变换信息，从而获取机器人在里程计odom坐标系下的位姿信息
class OdomTFListener(Node):
    def __init__(self):
        """
        初始化OdomTFListener节点
        """
        super().__init__('odom_tf_listener')
        # 创建TF变换的缓冲区，用于缓存坐标变换信息
        self.buffer = Buffer()
        # 创建TF变换监听器，将缓冲区和当前节点传入
        self.listener = TransformListener(self.buffer, self)
        # 创建定时器，每隔1秒触发一次获取坐标变换的操作，可根据实际需求调整时间间隔
        self.timer = self.create_timer(1.0, self.get_transform)

    def get_transform(self):
        """
        获取从'odom'坐标系到'base_footprint'坐标系的坐标变换信息，并提取位姿信息进行输出
        若获取过程出现异常，则进行相应的错误提示
        """
        try:
            # 尝试从缓冲区查找从'odom'坐标系到'base_footprint'坐标系的坐标变换
            # 使用时间相关参数来指定查找的时间点以及超时时间
            # rclpy.time.Time(seconds=0)表示获取最新可用的变换信息
            # rclpy.time.Duration(seconds=1)表示等待1秒超时，如果1秒内未获取到则抛出异常
            tf = self.buffer.lookup_transform(
                'odom', 'base_footprint', rclpy.time.Time(seconds=0), rclpy.time.Duration(seconds=1))
            transform = tf.transform

            # 提取平移信息（x, y, z坐标）
            x = transform.translation.x
            y = transform.translation.y
            z = transform.translation.z

            # 提取旋转信息（四元数转换为欧拉角）
            rotation_q = [
                transform.rotation.x,
                transform.rotation.y,
                transform.rotation.z,
                transform.rotation.w
            ]
            (roll, pitch, yaw) = euler_from_quaternion(rotation_q)

            # 将弧度制的欧拉角转换为角度制，方便阅读（角度 = 弧度 * 180 / π）
            roll_deg = roll * 180 / math.pi
            pitch_deg = pitch * 180 / math.pi
            yaw_deg = yaw * 180 / math.pi

            # 格式化输出位姿信息，保留一定的小数位数，使显示更清晰美观
            self.get_logger().info(
                f"位姿信息（基于里程计odom坐标系）：\n"
                f"  平移：x={x:.3f}, y={y:.3f}, z={z:.3f}（单位：米）\n"
                f"  旋转（欧拉角）：roll={roll_deg:.3f}°, pitch={pitch_deg:.3f}°, yaw={yaw_deg:.3f}°\n"
            )
        
        except Exception as e:
            # 捕获其他未预期的异常情况，输出通用的错误提示，方便排查问题
            self.get_logger().warn(f'获取从odom到base_footprint的坐标变换时出现未知错误，原因: {str(e)}')


def main():
    """
    主函数，用于初始化ROS 2环境，创建OdomTFListener节点并运行节点循环，最后关闭ROS 2环境
    """
    rclpy.init()
    node = OdomTFListener()
    rclpy.spin(node)
    rclpy.shutdown()


if __name__ == '__main__':
    main()
